//
// Created by An on 2025/7/4.
//
#include "start_task.h"
#include "cmsis_os.h"
#include "gpio.h"
#include "tim.h"
#include "usart.h"
#include "i2c.h"

#include <stdio.h>
#include <string.h>

#include "motor.h"
#include "tof.h"

#define  ARRAY_SIZE 6

void reorderArray(uint8_t *array) {
  uint8_t temp[ARRAY_SIZE];
  uint8_t *start = NULL;
  uint8_t *end = NULL;

  // 查找0xAA和0xBB的位置
  for (int i = 0; i < ARRAY_SIZE; i++) {
    if (array[i] == 0xAA) {
      start = &array[i];
    }
    if (array[i] == 0xBB) {
      end = &array[i];
    }
  }

  // 如果没有找到0xAA或0xBB，说明数组不合法
  if (start == NULL || end == NULL) {
    //printf("Invalid array: Missing 0xAA or 0xBB\n");
    array[0] = 0xCC;
    return;
  }

  // 如果0xBB在0xAA之前，说明顺序错误，需要重排
  if (end < start) {
    // 将0xAA到数组末尾的部分复制到临时数组
    memcpy(temp, start, ARRAY_SIZE - (start - array));
    // 将数组开头到0xBB的部分复制到临时数组的后半部分
    memcpy(temp + (ARRAY_SIZE - (start - array)), array, end - array + 1);

    // 将临时数组复制回原数组
    memcpy(array, temp, ARRAY_SIZE);
  }
}

bool dis_updated;
int16_t dis1, dis2;

void get_distance(const uint8_t *array) {
  dis1 = (int16_t) ((array[1] << 8) & 0xFF00 | (array[2]) & 0xFF);
  dis2 = (int16_t) ((array[3] << 8) & 0xFF00 | (array[4]) & 0xFF);
}

//所有的任务声明在这里
void TASK_CreateTask(void *pv);

void TASK_TestTask(void *pv);

void TASK_MotorUpdateTask(void *pv);

void TASK_LoopTask(void *pv);

void TASK_DashTask(void *pv);

//每个任务对应一个句柄
TaskHandle_t task_create_handle;
TaskHandle_t test_task_handle;
TaskHandle_t motor_update_task_handle;
TaskHandle_t loop_task_handle;
TaskHandle_t dash_task_handle;


//起始任务
void TASK_StartInit() {
  BaseType_t xReturn;
  taskENTER_CRITICAL();
  xReturn = xTaskCreate((TaskFunction_t) TASK_CreateTask,
                        "CreateTask",
                        128,
                        (void *) NULL,
                        2,
                        &task_create_handle);
  taskEXIT_CRITICAL();
  if (pdFALSE != xReturn) {
    vTaskStartScheduler();
  }
}

//创建任务，所有的任务应该由这一个任务创建完成
void TASK_CreateTask(void *pv) {
  BaseType_t xReturn;
  taskENTER_CRITICAL();
  xTaskCreate((TaskFunction_t) TASK_MotorUpdateTask,
              "MotorUpdateTask",
              512,
              (void *) NULL,
              5,
              &motor_update_task_handle);
  xTaskCreate((TaskFunction_t) TASK_LoopTask,
              "LoopTask",
              512,
              (void *) NULL,
              7,
              &loop_task_handle);
  xTaskCreate((TaskFunction_t) TASK_DashTask,
              "DashTask",
              512,
              (void *) NULL,
              5,
              &dash_task_handle);
  xReturn = xTaskCreate((TaskFunction_t) TASK_TestTask,
                        "TestTask",
                        512,
                        (void *) NULL,
                        4,
                        &test_task_handle);

  taskEXIT_CRITICAL();
  if (pdFALSE != xReturn) {
    vTaskDelete(task_create_handle);
  }

}

Motor_t motor_A;
Motor_t motor_B;
Motor_t motor_C;
Motor_t motor_D;


void move(int32_t y, int32_t x, int32_t theta) {
  motor_A.target_position += (-y + x + theta);
  motor_B.target_position += (y + x + theta);
  motor_C.target_position += (y - x + theta);
  motor_D.target_position += (-y - x + theta);
}

void TASK_MotorUpdateTask(void *argument) {
  const TickType_t period_ticks = pdMS_TO_TICKS(10);  // 每10ms更新
  const float delta_time = 0.01f;  // 对应10ms（单位：秒）
  TickType_t lastWakeTime = xTaskGetTickCount();

  for (;;) {
    vTaskDelayUntil(&lastWakeTime, period_ticks);

    // 更新编码器与速度
    Motor_EstimateSpeed(&motor_A, delta_time);
    Motor_EstimateSpeed(&motor_B, delta_time);
    Motor_EstimateSpeed(&motor_C, delta_time);
    Motor_EstimateSpeed(&motor_D, delta_time);

    Motor_PositionControl(&motor_A);
    Motor_PositionControl(&motor_B);
    Motor_PositionControl(&motor_C);
    Motor_PositionControl(&motor_D);

    // 执行速度环闭环控制
    Motor_SpeedControl(&motor_A);
    Motor_SpeedControl(&motor_B);
    Motor_SpeedControl(&motor_C);
    Motor_SpeedControl(&motor_D);
  }
}
#define DASHTIME 3000
bool updated;
uint8_t rx_message[6];

void TASK_LoopTask(void *pv) {
  Motors_Init();

  const TickType_t period_ticks = pdMS_TO_TICKS(20);  // 每50ms更新
  TickType_t lastWakeTime = xTaskGetTickCount();


  float delta_theta;
  float total_delta_theta=0;
  float delta_x;
  int offset = 10;

  //vTaskDelay(DASHTIME);
  while (true) {

    vTaskDelayUntil(&lastWakeTime, period_ticks);
    reorderArray(rx_message);
    get_distance(rx_message);
    if (dis1 < 2000 && dis2 < 2000 && updated) {
      delta_theta = 0.2f * (dis2 - dis1 + offset);
      if (delta_theta > 30) {
        delta_theta = 30;
      }
      if (delta_theta < -30) {
        delta_theta = -30;
      }
//      total_delta_theta+=delta_theta;
//      if (total_delta_theta > 200) {
//        total_delta_theta = 200;
//        delta_theta=0;
//      }
//      if (total_delta_theta < -200) {
//        total_delta_theta=-200;
//        delta_theta = 0;
//      }

      delta_x = 0.1f * (200 - (dis2 + dis1) / 2);

      updated = false;
      move(0, int(delta_x), int(delta_theta));
      //move(0, int(delta_x), 0);
    }

  }
}

void TASK_DashTask(void *pv){
  //这部分用于冲刺
  vTaskDelay(200);
  motor_A.position_pid.output_min=-800;
  motor_A.position_pid.output_max=800;
  motor_B.position_pid.output_min=-800;
  motor_B.position_pid.output_max=800;
  motor_C.position_pid.output_min=-800;
  motor_C.position_pid.output_max=800;
  motor_D.position_pid.output_min=-800;
  motor_D.position_pid.output_max= 800;
  move(1800, 0, 0);
  vTaskDelay(DASHTIME);
  //降低速度
  motor_A.position_pid.output_min=-300;
  motor_A.position_pid.output_max=300;
  motor_B.position_pid.output_min=-300;
  motor_B.position_pid.output_max=300;
  motor_C.position_pid.output_min=-300;
  motor_C.position_pid.output_max=300;
  motor_D.position_pid.output_min=-300;
  motor_D.position_pid.output_max=300;
  while (true){
    move(-700, 0, 0);
    vTaskDelay(2500);

    move(300, 0, 0);
    vTaskDelay(1500);
    move(400, 0, 0);
    vTaskDelay(1500);
  }
}

char tx_message[50];
//想写点什么东西测试一下的话可以写在这里
void TASK_TestTask(void *pv) {

  HAL_UART_Receive_IT(&huart1, (uint8_t *) rx_message, sizeof rx_message);

  while (true) {

//    sprintf(tx_message, "dis1:\t%d,dis2:\t%d", dis1, dis2 );
//    HAL_UART_Transmit(&huart6, (uint8_t *) tx_message, sizeof tx_message, HAL_MAX_DELAY);
//    HAL_GPIO_TogglePin(LED_GPIO_Port, LED_Pin);
    vTaskDelay(1000);
  }
}

void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart) {
  if (huart = &huart6) {
    updated = true;
    HAL_UART_Receive_IT(&huart1, (uint8_t *) rx_message, sizeof rx_message);
  }
}

